from machine import Pin,PWM,ADC
from NeoPID import PID

class Wave_filtering():
    def __init__(self,k):
        self.a=[0,0,0]
        self.c=k
    def filtering(self,d):
        s=0
        for i in self.a:
            s = s + i
        g=s/3
        if d<g-self.c:
            o = self.a[0]-self.c
        elif d>g+self.c:
            o = self.a[0]+self.c
        else:
            o = d
        self.a[2]=self.a[1]
        self.a[1]=self.a[0]
        self.a[0]=o
        return o
    
class ADC_sensor:
    def __init__(self):
        self.in1 = ADC(Pin(2))
        self.in2 = ADC(Pin(3))
        self.in3 = ADC(Pin(5))
        self.in4 = ADC(Pin(4))
        self.in1.atten(ADC.ATTN_11DB)  # 这里配置测量量程为3.3V
        self.in2.atten(ADC.ATTN_11DB)
        self.in3.atten(ADC.ATTN_11DB)
        self.in4.atten(ADC.ATTN_11DB)
        self.wf1=Wave_filtering(500)
        self.wf2=Wave_filtering(500)
        self.wf3=Wave_filtering(500)
        self.wf4=Wave_filtering(500)
           
    def read_value(self):
        v1=self.in1.read()+0 # 消除零点偏差
        v2=self.in2.read()+690
        v3=self.in3.read()+0
        v4=self.in4.read()+0
        v1=self.wf1.filtering(v1)
        v2=self.wf2.filtering(v2)
        v3=self.wf3.filtering(v3)
        v4=self.wf4.filtering(v4)
        return (v1,v2,v3,v4)
if __name__ == '__main__':
    sen=ADC_sensor()
    pid=PID(0.7,0.2,0.1,input_lim=8191,output_lim=1000)
    while True:
        adc_value=sen.read_value()
        err=adc_value[0]-adc_value[1]
        adj=pid.get_adj_value(err)
        print (adj)